#include "cordic.h"
#include <stdio.h>

const char neg_atantable [] = {0x23, 0x10, 0x08, 0x04, 0x04, 0x02, 0x01, 0x01};
const char zro_atantable [] = {0x20, 0x10, 0x08, 0x04, 0x02, 0x01, 0x01, 0x00};
const char pos_atantable [] = {0x32, 0x1e, 0x10, 0x08, 0x04, 0x02, 0x01, 0x00};

const char neg_K = 0x4d;
const char zro_K = 0x40;
const char pos_K = 0x27;

const char neg_Fi [] = {1, 2, 3, 4, 4, 5, 6, 7};
const char zro_Fi [] = {1, 2, 3, 4, 5, 6, 7, 8};
const char pos_Fi [] = {0, 1, 2, 3, 4, 5, 6, 7};

void cordic(CordicRecord* record, char coordinate, unsigned char approxmode){
	unsigned char i = 0;
	char tempX = 0x00;
	char tempY = 0x00;
	char tempZ = 0x00;
	
	switch (approxmode){
		case 0: /*y -> 0*/
			switch (coordinate){
				case -1: /*m = -1*/
					for (i = 0; i < 8; ++i){
						if (record->y >= 0x00){
							tempX = record->x - (record->y >> neg_Fi[i]);
							tempY = record->y - (record->x >> neg_Fi[i]);
							tempZ = record->z + neg_atantable[i];
							record->x = tempX;
							record->y = tempY;
							record->z = tempZ;
						}else{
							tempX = record->x + (record->y >> neg_Fi[i]);
							tempY = record->y + (record->x >> neg_Fi[i]);
							tempZ = record->z - neg_atantable[i];
							record->x = tempX;
							record->y = tempY;
							record->z = tempZ;
						}
					}
					break;
				case 0: /*m = 0*/
					for (i = 0; i < 8; ++i){
						if (record->y >= 0x00){
							tempY = record->y - (record->x >> zro_Fi[i]);
							tempZ = record->z + zro_atantable[i];
							record->y = tempY;
							record->z = tempZ;
						}else{
							tempY = record->y + (record->x >> zro_Fi[i]);
							tempZ = record->z - zro_atantable[i];
							record->y = tempY;
							record->z = tempZ;
						}
					}
					break;
				case 1: /*m = 1*/
					for (i = 0; i < 8; ++i){
						if (record->y >= 0x00){
							tempX = record->x + (record->y >> pos_Fi[i]);
							tempY = record->y - (record->x >> pos_Fi[i]);
							tempZ = record->z + pos_atantable[i];
							record->x = tempX;
							record->y = tempY;
							record->z = tempZ;
						}else{
							tempX = record->x - (record->y >> pos_Fi[i]);
							tempY = record->y + (record->x >> pos_Fi[i]);
							tempZ = record->z - pos_atantable[i];
							record->x = tempX;
							record->y = tempY;
							record->z = tempZ;
						}
					}
					break;
			}
			break;
		case 1: /*z -> 0*/
			switch (coordinate){
				case -1: /*m = -1*/
					for (i = 0; i < 8; ++i){
						if (record->z < 0){
							tempX = record->x - (record->y >> neg_Fi[i]);
							tempY = record->y - (record->x >> neg_Fi[i]);
							tempZ = record->z + neg_atantable[i];
							record->x = tempX;
							record->y = tempY;
							record->z = tempZ;
						}else{
							tempX = record->x + (record->y >> neg_Fi[i]);
							tempY = record->y + (record->x >> neg_Fi[i]);
							tempZ = record->z - neg_atantable[i];
							record->x = tempX;
							record->y = tempY;
							record->z = tempZ;
						}
					}
					break;
				case 0: /*m = 0*/
					for (i = 0; i < 8; ++i){
						if (record->z < 0){
							tempY = record->y - (record->x >> zro_Fi[i]);
							tempZ = record->z + zro_atantable[i];
							record->y = tempY;
							record->z = tempZ;
						}else{
							tempY = record->y + (record->x >> zro_Fi[i]);
							tempZ = record->z - zro_atantable[i];
							record->y = tempY;
							record->z = tempZ;
						}
					}
					break;
				case 1: /*m = 1*/
					for (i = 0; i < 8; ++i){
						if (record->z < 0){
							tempX = record->x + (record->y >> pos_Fi[i]);
							tempY = record->y - (record->x >> pos_Fi[i]);
							tempZ = record->z + pos_atantable[i];
							record->x = tempX;
							record->y = tempY;
							record->z = tempZ;
						}else{
							tempX = record->x - (record->y >> pos_Fi[i]);
							tempY = record->y + (record->x >> pos_Fi[i]);
							tempZ = record->z - pos_atantable[i];
							record->x = tempX;
							record->y = tempY;
							record->z = tempZ;
						}
					}
					break;
			}
			break;
	}
}
